#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from sensor_msgs.msg import Range
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
import serial
import time

class UltrasonicSensorNode(Node):
    def __init__(self):
        super().__init__('ultrasonic_sensor_node')
        
        # 声明参数
        self.declare_parameter('port', '/dev/ttyUSB_US')
        self.declare_parameter('baudrate', 9600)
        self.declare_parameter('update_frequency', 1.0)  # 降低频率减少丢包
        self.declare_parameter('frame_id', 'laser')
        self.declare_parameter('timeout', 0.1)
        self.declare_parameter('trigger_command', '0x01')
        self.declare_parameter('sensor_x', 0.1)
        self.declare_parameter('sensor_y', 0.0)
        self.declare_parameter('sensor_z', 0.2)
        self.declare_parameter('min_valid_distance', 2.0)  # 最小有效距离(cm)
        self.declare_parameter('max_valid_distance', 400.0)  # 最大有效距离(cm)
        
        # 获取参数
        self.port = self.get_parameter('port').value
        self.baudrate = self.get_parameter('baudrate').value
        self.update_frequency = self.get_parameter('update_frequency').value
        self.frame_id = self.get_parameter('frame_id').value
        self.timeout = self.get_parameter('timeout').value
        self.trigger_cmd = self.get_parameter('trigger_command').value
        self.sensor_x = self.get_parameter('sensor_x').value
        self.sensor_y = self.get_parameter('sensor_y').value
        self.sensor_z = self.get_parameter('sensor_z').value
        self.min_distance = self.get_parameter('min_valid_distance').value
        self.max_distance = self.get_parameter('max_valid_distance').value
        
        # 初始化串口和数据缓冲区
        self.ser = None
        self._init_serial_port()
        self.data_buffer = bytearray()
        
        # 帧解析状态
        self.FRAME_STATE_IDLE = 0
        self.FRAME_STATE_IN_FRAME = 1
        self.frame_state = self.FRAME_STATE_IDLE
        
        # 创建发布器
        self.distance_publisher = self.create_publisher(
            Float32, 
            'ultrasonic_distance', 
            10
        )
        
        self.range_publisher = self.create_publisher(
            Range, 
            'ultrasonic_range', 
            10
        )
        
        # 创建TF广播器
        self.tf_broadcaster = TransformBroadcaster(self)
        
        # 初始化Range消息
        self._init_range_message()
        
        # 初始化TF变换
        self._init_tf_transform()
        
        # 创建定时器
        self.timer = self.create_timer(1.0 / self.update_frequency, self.timer_callback)
        
        self.get_logger().info(f"超声波传感器节点启动，过滤范围: {self.min_distance}-{self.max_distance}cm")

    def _init_serial_port(self):
        """初始化串口连接"""
        try:
            self.ser = serial.Serial(
                port=self.port,
                baudrate=self.baudrate,
                timeout=self.timeout,
                write_timeout=0.5
            )
            self.get_logger().info(f"成功连接到串口: {self.port}")
        except serial.SerialException as e:
            self.get_logger().fatal(f"串口连接失败: {e}")
            exit(1)

    def _send_trigger_command(self):
        """发送触发指令到传感器"""
        if not self.trigger_cmd:
            return
            
        try:
            if self.trigger_cmd.startswith('0x'):
                cmd_bytes = bytes.fromhex(self.trigger_cmd[2:])
            else:
                cmd_bytes = bytes([int(self.trigger_cmd)])
            
            self.ser.write(cmd_bytes)
            self.get_logger().debug(f"发送触发指令: {self.trigger_cmd} → {cmd_bytes.hex()}")
        except Exception as e:
            self.get_logger().error(f"发送指令失败: {e}")

    def _init_range_message(self):
        """初始化Range消息"""
        self.range_msg = Range()
        self.range_msg.header.frame_id = self.frame_id
        self.range_msg.radiation_type = Range.ULTRASOUND
        self.range_msg.field_of_view = 0.349  # 约20度
        self.range_msg.min_range = self.min_distance / 100.0  # 转换为米
        self.range_msg.max_range = self.max_distance / 100.0  # 转换为米

    def _init_tf_transform(self):
        """初始化传感器的TF变换"""
        self.transform = TransformStamped()
        self.transform.header.frame_id = 'base_link'
        self.transform.child_frame_id = self.frame_id
        self.transform.transform.translation.x = self.sensor_x
        self.transform.transform.translation.y = self.sensor_y
        self.transform.transform.translation.z = self.sensor_z
        self.transform.transform.rotation.x = 0.0
        self.transform.transform.rotation.y = 0.0
        self.transform.transform.rotation.z = 0.0
        self.transform.transform.rotation.w = 1.0

    def timer_callback(self):
        """定时器回调 - 循环接收和处理数据"""
        try:
            if self.trigger_cmd:
                self._send_trigger_command()
            
            if self.ser.in_waiting > 0:
                new_data = self.ser.read(self.ser.in_waiting)
                self.data_buffer.extend(new_data)
                self.get_logger().debug(f"接收数据: {new_data.hex()}")
                self._process_data()
            
        except serial.SerialException as e:
            self.get_logger().error(f"串口异常: {e}")
            self._init_serial_port()
        except Exception as e:
            self.get_logger().error(f"处理数据时出错: {e}")

    def _process_data(self):
        """处理接收到的数据缓冲区"""
        if not self.data_buffer:
            return
            
        while len(self.data_buffer) > 0:
            if self.frame_state == self.FRAME_STATE_IDLE:
                start_index = self.data_buffer.find(b'\xff')
                if start_index == -1:
                    self.data_buffer = bytearray()
                    return
                    
                if start_index > 0:
                    self.data_buffer = self.data_buffer[start_index:]
                
                self.frame_state = self.FRAME_STATE_IN_FRAME
                self.get_logger().debug("进入帧接收状态")
                
            elif self.frame_state == self.FRAME_STATE_IN_FRAME:
                end_index = self.data_buffer.find(b'\xff', 1)
                
                if end_index == -1:
                    return
                    
                frame_data = self.data_buffer[1:end_index]
                
                distance = self._parse_frame(frame_data)
                if distance is not None:
                    self._publish_data(distance)
                
                self.data_buffer = self.data_buffer[end_index:]
                self.frame_state = self.FRAME_STATE_IDLE
                self.get_logger().debug("返回空闲状态")

    def _parse_frame(self, frame_data):
        """解析帧数据"""
        self.get_logger().debug(f"解析帧: {frame_data.hex()}")
        
        if len(frame_data) < 2:
            #self.get_logger().warn(f"帧长度不足: {len(frame_data)} 字节")
            return None
            
        # 解析距离数据
        distance_high = frame_data[0]
        distance_low = frame_data[1]
        distance_value = (distance_high << 8) | distance_low
        distance_cm = distance_value / 10.0
        
        return distance_cm

    def _publish_data(self, distance):
        """发布数据前进行过滤"""
        # 过滤无效距离（0或超出范围）
        if distance <= self.min_distance or distance >= self.max_distance:
            self.get_logger().warn(f"过滤无效距离: {distance}cm (有效范围: {self.min_distance}-{self.max_distance}cm)")
            return
            
        # 发布TF变换
        self.transform.header.stamp = self.get_clock().now().to_msg()
        self.tf_broadcaster.sendTransform(self.transform)
        
        # 发布距离消息
        distance_msg = Float32()
        distance_msg.data = distance
        self.distance_publisher.publish(distance_msg)
        
        # 发布Range消息
        self.range_msg.header.stamp = self.get_clock().now().to_msg()
        self.range_msg.range = distance / 100.0
        self.range_publisher.publish(self.range_msg)
        
        self.get_logger().info(f"发布有效距离: {distance:.2f}cm")

def main(args=None):
    rclpy.init(args=args)
    node = UltrasonicSensorNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if node.ser and node.ser.is_open:
            node.ser.close()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

